#include <SD.h>
#include <avr/pgmspace.h>
#include <GPS.h>

#define smspower 2
#define rtty1 6     //RTTY pins
#define rtty2 5     //RTTY pins
#define LEDgreen 8  //Green LED on GPS shield
#define LEDred 9    //Red LED on GPS shield
#define SDselect 10 //Pin to select SD card with SPI
String GPSstring = "";
String time, type, fix, longitude, latitude,altitude;
double alt;
int counts = 1;

GPS TRIMBLE;


void setup(){

  pinMode(smspower,OUTPUT);
  pinMode(SDselect,OUTPUT); //Chip select pin defined as output
  pinMode(LEDgreen,OUTPUT);
  pinMode(LEDred,OUTPUT);
  pinMode(rtty1, OUTPUT);
  pinMode(rtty2, OUTPUT);


  if(!SD.begin(SDselect)){
    Serial.println("ERROR");
  } //Initialize SD card

  digitalWrite(smspower,LOW);
  Serial.begin(4800); //Baud rate for serial communication with GPS

}

void loop(){
  GPSstring = "";
  Serial.print("a");
  char c;

  if (Serial.available()){
  
    c = Serial.read();
    while (c != '$'){
      if (Serial.available()){
        c = Serial.read();
      }
      Serial.print("b");
    }

    GPSstring += c;
    while (c != '\n'){
      if (Serial.available()){
        c = Serial.read();
        GPSstring += c;
        if (c == '$' || c == 'V'){
          return;
        }
      }
      Serial.print("c");
    }
    Serial.print("C");
    TRIMBLE.collect(GPSstring);
    Serial.println("d");
    Serial.println(GPSstring);
  }


  fix = TRIMBLE.getdata(7);

  if (fix == ",1" && TRIMBLE.checksum()){
    Serial.print("e");
    type = TRIMBLE.getdata(1);
    time = TRIMBLE.getdata(2);
    latitude = TRIMBLE.getdata(3);
    longitude = TRIMBLE.getdata(5);
    altitude = TRIMBLE.getdata(10);
    alt = TRIMBLE.getdata_number(10);
    time = format_time(&time[0]);
    Serial.print("f");
    
    
    if (TRIMBLE.getdata(4)==",S"){
      latitude[0] = '-';
    }
    else {
      latitude[0] = '+';
    }
    Serial.print("g");
    if (TRIMBLE.getdata(6) ==",E"){
      longitude[0] = '+';
    }
    else {
      longitude[0] = '-';  
    }
    Serial.print("h");
  }

  String transmission = "$$voyeurv,";
  transmission += counts + time + ',' + latitude + ',' + longitude + altitude;
  Serial.print("H");
  String checksum = gps_xor_checksum(&transmission[0]);
  transmission += '*' + checksum + '\n';
  Serial.print("I");
  String logstring = "";
  logstring += time.substring(1) + ',' + longitude + ',' + latitude + altitude + '\n';
  Serial.print("Y");

  if (transmission.length()>30){
    Serial.print("i");
    SDcard_write(&logstring[0]); //Writes data to SD card. References the first char of logstring.


    counts++;
    if (counts > 3 || alt < 5000){
      sendsms(&transmission[0]);
      //counts = 0;
    }

    rtty_txstring(&transmission[0]);
  }
  /*Serial.print("j");
  Serial.flush();
  delay(1000);*/
  Serial.println("k");
}



